#include "motorcontroller.h"

/**
 * Type definitions
 */
typedef		bool	bit;

/**
 * Constants
 */
static const	bit				MOTOR_LEFT_DIRECTION	=	0;
static const	bit				MOTOR_RIGHT_DIRECTION	=	1;
static const	bit				MOTOR_LEFT_ENABLE		=	2;
static const	bit				MOTOR_RIGHT_ENABLE		=	3;

static const	int				SLEEP_TIME				=	100;
static const	unsigned char	SPEED_FORWARD			=	255;
static const	unsigned char	SPEED_BACKWARD			=	0;

/*
 * Fields
 */
static	GPIO			*gpio;

/**
 * Constructor.
 */
MotorController::MotorController()
{
	// Prepare the GPIO ports for output
	gpio->InstallOutput(MOTOR_LEFT_DIRECTION);
	gpio->InstallOutput(MOTOR_RIGHT_DIRECTION);
	gpio->InstallOutput(MOTOR_LEFT_ENABLE);
	gpio->InstallOutput(MOTOR_RIGHT_ENABLE);

	// Disable the motors
	this->Stop();
}

/**
 * Destructor.
 */
MotorController::~MotorController()
{
	// Stop the motors
	this->Stop();
}

/**
 * Move forward.
 */
void MotorController::MoveForward()
{
	// Drive
	//gpio->PortHigh(MOTOR_LEFT_DIRECTION);
	//gpio->PortHigh(MOTOR_RIGHT_DIRECTION);
	gpio->PortWrite(MOTOR_LEFT_DIRECTION, SPEED_FORWARD);
	gpio->PortWrite(MOTOR_RIGHT_DIRECTION, SPEED_FORWARD);
	gpio->PortHigh(MOTOR_LEFT_ENABLE);
	gpio->PortHigh(MOTOR_RIGHT_ENABLE);

	// Stop
	usleep(SLEEP_TIME);
	this->Stop();
}

/**
 * Move backward.
 */
void MotorController::MoveBackward()
{
	// Drive
	//gpio->PortLow(MOTOR_LEFT_DIRECTION);
	//gpio->PortLow(MOTOR_RIGHT_DIRECTION);
	gpio->PortWrite(MOTOR_LEFT_DIRECTION, SPEED_BACKWARD);
	gpio->PortWrite(MOTOR_RIGHT_DIRECTION, SPEED_BACKWARD);
	gpio->PortHigh(MOTOR_LEFT_ENABLE);
	gpio->PortHigh(MOTOR_RIGHT_ENABLE);

	// Stop
	usleep(SLEEP_TIME);
	this->Stop();
}

/**
 * Go left.
 */
void MotorController::GoLeft()
{
	// Drive
	//gpio->PortHigh(MOTOR_LEFT_DIRECTION);
	//gpio->PortLow(MOTOR_RIGHT_DIRECTION);
	gpio->PortWrite(MOTOR_LEFT_DIRECTION, SPEED_BACKWARD);
	gpio->PortWrite(MOTOR_RIGHT_DIRECTION, SPEED_FORWARD);
	gpio->PortHigh(MOTOR_LEFT_ENABLE);
	gpio->PortHigh(MOTOR_RIGHT_ENABLE);

	// Stop
	usleep(SLEEP_TIME);
	this->Stop();
}

/**
 * Go right.
 */
void MotorController::GoRight()
{
	// Drive
	//gpio->PortLow(MOTOR_LEFT_DIRECTION);
	//gpio->PortHigh(MOTOR_RIGHT_DIRECTION);
	gpio->PortWrite(MOTOR_LEFT_DIRECTION, SPEED_FORWARD);
	gpio->PortWrite(MOTOR_RIGHT_DIRECTION, SPEED_BACKWARD);
	gpio->PortHigh(MOTOR_LEFT_ENABLE);
	gpio->PortHigh(MOTOR_RIGHT_ENABLE);

	// Stop
	usleep(SLEEP_TIME);
	this->Stop();
}

/**
 * Stop.
 */
void MotorController::Stop()
{
	gpio->PortLow(MOTOR_LEFT_ENABLE);
	gpio->PortLow(MOTOR_RIGHT_ENABLE);
}































